import * as THREE from "../../../../modules/three/three.module.js";
import {WorldConfig} from "../CommonConsts.js";
import PlaneRefVO, {computeSizeInFrame, computeSizeInPlane} from "../../model/PlaneRefVO.js";
import common_vertexGlsl from "../shader/common_vertex.glsl.js";
import dot_grid_fragmentGlsl from "../shader/dot_grid_fragment.glsl.js";

const unitY = new THREE.Vector3(0, 1, 0); // = new THREE.Vector3(0, 0, 1);
const unitZ = new THREE.Vector3(0, 0, 1);

const planeGeometry = new THREE.PlaneGeometry(1, 1);
// planeGeometry.applyMatrix4(new THREE.Matrix4().makeRotationX(-Math.PI / 2))
const colorDefault = '#1164ff'
const colorActive = '#25a23f'
const mtl = new THREE.MeshBasicMaterial({
  color: colorDefault,
  transparent: true,
  depthWrite: true,
  opacity: 0.4,
  side: THREE.DoubleSide,
});
const lineMtl = new THREE.LineBasicMaterial({ color: '#185598' });
const edgeGeometry = new THREE.EdgesGeometry(planeGeometry);
const pointOnPlane = new THREE.Vector3()

// 辅助平面激活或选中后 所使用的材质
function getDotGridMaterial() {
  const { r, g, b } = mtl.color;
  const uniforms = {
    dotColor: { value: new THREE.Vector4(r, g, b, 1) },
    repeat: {
      value: new THREE.Vector2(
        20,
        (20 * window.innerHeight) / window.innerWidth,
      ),
    },
  };
  const dotGridMaterial = new THREE.ShaderMaterial({
    uniforms: uniforms,
    vertexShader: common_vertexGlsl,
    fragmentShader: dot_grid_fragmentGlsl,
    side: THREE.DoubleSide,
    transparent: true,
    depthTest: false,
    depthWrite: false,
  });
  return dotGridMaterial;
}

export default class PlaneRefMesh extends THREE.Mesh {
  /**
   * @type {PlaneRefVO}
   */
  vo;
  /**
   * @type {THREE.Mesh}
   */
  selectionMesh;
  /**
   * 属于世界坐标系的坐标
   * @type {THREE.Vector3[]}
   */
  corners = [];
  edges

  constructor(geometry = planeGeometry, material = mtl.clone()) {
    super(geometry, material);
    const edges = new THREE.LineSegments(edgeGeometry, lineMtl);
    this.edges = edges;
    edges.visible = false;
    this.add(edges);
    this.selectionMesh = new THREE.Mesh(geometry, getDotGridMaterial());
    this.selectionMesh.visible = false;
    this.add(this.selectionMesh);
    this.renderOrder = WorldConfig.order_plane_ref
    this.selectionMesh.renderOrder = WorldConfig.order_plane_ref
  }

  toggleSelected(selected) {
    // this.material.opacity = selected ? 0.0 : 0.4;
    this.material.colorWrite = !selected;
    this.edges.visible = selected;
    this.selectionMesh.visible = selected;
  }

  toggleActive(active) {
    const clr = active ? colorActive : colorDefault
    const color = new THREE.Color().setStyle(clr)
    const {r, g, b} = color
    const mtl = this.selectionMesh.material
    mtl.uniforms.dotColor.value.set(r, g, b, 1)
  }

  updatePlane() {
    const plane = this.vo.plane;
    plane.setFromNormalAndCoplanarPoint(plane.normal, this.position);
  }

  initPlaneRef(vo) {
    this.vo = vo;
    this.position.copy(vo.pos);
    this.updateSize(vo.width, vo.height);
    if (vo.rot.length) {
      const [x, y, z] = vo.rot
      this.rotation.set(x, y, z)
    } else {
      this.quaternion.setFromUnitVectors(unitZ, vo.plane.normal);
    }
    this.initCorners();
    return this;
  }

  updateVO() {
    const [x, y, z] = this.rotation
    this.vo.rot = [x, y, z]
  }

  updateSize(width, height) {
    this.scale.x = width;
    this.scale.y = height;
    this.vo.width = width;
    this.vo.height = height;
    const repeat = this.selectionMesh.material.uniforms.repeat.value;
    repeat.set(width * 2, height * 2);
  }

  updateDistanceToWall(distance) {
    const plane = this.vo.plane;
    const oldDistance = plane.distanceToPoint(this.vo.pos);
    // const planeCenter = plane.coplanarPoint(new THREE.Vector3());
    const offset = plane.normal.clone().multiplyScalar(distance - oldDistance);
    this.vo.pos.add(offset);
    this.position.copy(this.vo.pos);
  }

  updateOrientation(normal) {
    this.quaternion.setFromUnitVectors(unitZ, normal);
    this.vo.plane.normal.copy(normal);
  }

  updateBySamplePoints(samplePoints, eyePos) {
    const center = new THREE.Vector3();
    samplePoints.forEach(it => center.add(it));
    center.divideScalar(samplePoints.length);
    const plane = this.vo.plane;
    // const oldDistance = plane.distanceToPoint(this.vo.pos);
    const planeParameters = calculatePlane(samplePoints);
    const normal = new THREE.Vector3(...planeParameters.slice(0, 3));
    const len = normal.length();
    const distance = 1.0 / len * planeParameters[3];
    // console.log('updateBySamplePoints, normal: ', normal.toArray())
    normal.normalize();
    plane.normal.copy(normal);
    plane.constant = distance;
    plane.projectPoint(center, pointOnPlane);
    if (PlaneRefVO.rotateOnlyZ) {
      normal.z = 0;
      normal.normalize();
      plane.setFromNormalAndCoplanarPoint(normal, pointOnPlane)
    }
    if (plane.distanceToPoint(eyePos) < 0) {
      normal.negate();
      plane.constant = -distance;
      plane.normal.copy(normal);
    }

    plane.projectPoint(center, this.vo.pos);
    this.vo.pos.add(plane.normal.clone().multiplyScalar(WorldConfig.building_plane_distance));
    this.position.copy(this.vo.pos);
    const ux = samplePoints[0].clone().sub(samplePoints[1]).normalize()
    if (PlaneRefVO.rotateOnlyZ) {
      ux.set(0, 0, 1)
    }
    const uz = plane.normal
    const uy = uz.clone().cross(ux).normalize()
    const mat = new THREE.Matrix4().makeBasis(ux, uy, uz).setPosition(this.position)
    const matInv = mat.clone().invert()
    this.quaternion.setFromRotationMatrix(mat)
    const size = computeSizeInFrame(samplePoints, matInv)
    // const size = computeSizeInPlane(samplePoints, plane);
    this.updateSize(size.x * 1.25, size.y * 1.25);
    this.vo.rot = [this.rotation.x, this.rotation.y, this.rotation.z]
    this.initCorners();
  }

  initCorners() {
    this.corners.length = 0;
    const hw = 0.5;
    const hh = 0.5;
    const v0 = new THREE.Vector3(-hw, 0, -hh);
    const v1 = new THREE.Vector3(+hw, 0, -hh);
    const v2 = new THREE.Vector3(+hw, 0, +hh);
    const v3 = new THREE.Vector3(-hw, 0, +hh);
    const corners = [v0, v1, v2, v3];
    this.updateMatrixWorld();
    corners.forEach((it) => it.applyMatrix4(this.matrixWorld));
    this.corners.push(...corners);
  }
}

export function createPlaneBySvd(points) {
  const planeParameters = calculatePlane(points);
  const normal = new THREE.Vector3(...planeParameters.slice(0, 3));
  const len = normal.length();
  const distance = 1.0 / len * planeParameters[3];
  const plane = new THREE.Plane(normal.normalize(), distance)
  return plane
}

function calculatePlane(vectors) {
  const points = vectors.map(it => it.toArray());
  const {bestPlane} = ransacPlaneFitting(points);
  return bestPlane;
}

function calculatePlaneAndResiduals0(vectors) {
  const points = vectors.map(it => it.toArray());
  const rows = points.length;
  const A = new Array(rows);
  for (let i = 0; i < rows; i++) {
    A[i] = points[i].slice();
    A[i].push(1);
  }

  // svd分解
  const svd = numeric.svd(A);

  const V = svd.V;

  const planeParameters = V.map(row => row[row.length - 1]); // 平面系数

  // 计算残差平方和
  const normal = planeParameters.slice(0, 3);
  const distances = numeric.div(numeric.abs(numeric.dot(A, planeParameters)), numeric.norm2(normal));
  const residualSumOfSquares = numeric.sum(numeric.pow(distances, 2));
  console.log('residualSumOfSquares: ', residualSumOfSquares)
  return planeParameters;
}

function cross(a, b) {
  return [
    a[1] * b[2] - a[2] * b[1],
    a[2] * b[0] - a[0] * b[2],
    a[0] * b[1] - a[1] * b[0]
  ];
}

function calculatePlaneAndResiduals(points) {

  // 构建齐次坐标
  const A = points.map(point => point.concat(1));

  // svd分解
  const svd = numeric.svd(A);

  const V = svd.V;

  const plane_coeffs = V.map(row => row[row.length - 1]); // 平面系数

  // 计算残差平方和
  const normal = plane_coeffs.slice(0, 3);
  const distances = numeric.div(numeric.abs(numeric.dot(A, plane_coeffs)), numeric.norm2(normal));
  const residual_sum_of_squares = numeric.sum(numeric.pow(distances, 2));

  return { plane_coeffs: plane_coeffs, residual_sum_of_squares: residual_sum_of_squares };
}

function ransacPlaneFitting(points, numIterations = 1000, distanceThreshold = 0.25, minInliers = 100) {
  let bestPlane = null;
  let maxInliers = 0;
  let bestResidual = Infinity;

  for (let i = 0; i < numIterations; i++) {
    let samplePoints;
    let isCollinear;
    do {
      // numeric库进行奇异值分解需要矩阵的行数大于等于列数，所以这里需要保证采样点的数量大于等于3
      const idx = numeric.random([4]).map(x => Math.floor(x * points.length));
      samplePoints = idx.map(index => points[index]);

      const v1 = numeric.sub(samplePoints[1], samplePoints[0]);
      const v2 = numeric.sub(samplePoints[2], samplePoints[0]);
      const crossProduct = cross(v1, v2);
      isCollinear = numeric.norm2(crossProduct) < 1e-8;
    } while (isCollinear); // 验证选取点集是否是共线的

    const { plane_coeffs, residual_sum_of_squares } = calculatePlaneAndResiduals(samplePoints);
    if (!plane_coeffs) {
      continue;
    }

    const A = points.map(point => point.concat(1));
    const normal = plane_coeffs.slice(0, 3);
    const distances = numeric.div(numeric.abs(numeric.dot(A, plane_coeffs)), numeric.norm2(normal));
    const inliers = distances.map(d => d < distanceThreshold);

    const inlierCount = inliers.filter(Boolean).length;
    if (inlierCount > maxInliers) {
      maxInliers = inlierCount;
      bestPlane = plane_coeffs;
      bestResidual = residual_sum_of_squares;
      // console.log("Best plane inlier:", inliers);
    }

    if (maxInliers > minInliers) {
      break;
    }
  }

  return { bestPlane, bestResidual };
}
